#include "SensorDataFileInput.h"
#include <iostream>
#include <process.h>

SensorDataFileInput::SensorDataFileInput (const std::string &name,
		char* dataFile, const std::string &intrinsicFile) :
		SensorDataInput (name, intrinsicFile), _hasNewFrame (false),
		_inStream (dataFile, std::ios::binary)
{
	if (!_inStream) {
		printf ("Cannot open the file %s", dataFile);
		throw (-1);
	}

	__sensorParameter.readFrom (_inStream);

	if (__sensorParameter.depthPixelWidth) {
		__depthMetaData = cvCreateImage (cvSize (
				__sensorParameter.frameWidth,
				__sensorParameter.frameHeight), IPL_DEPTH_16U, 1);
		cvZero (__depthMetaData);
	}
	if (__sensorParameter.rgbPixelWidth) {
		__rgbMetaData = cvCreateImage (cvSize (
				__sensorParameter.frameWidth,
				__sensorParameter.frameHeight), 8, 3);
		cvZero (__rgbMetaData);
	}

	_frameDataSize = __sensorParameter.frameSize * (
			__sensorParameter.depthPixelWidth + __sensorParameter.rgbPixelWidth);
	_buf = new char[_frameDataSize * 2];
	_frontBuf = _buf + _frameDataSize;
	_backBuf = _buf;
	_frontBufReady = CreateSemaphore( 
			NULL,           // default security attributes
			0,  // initial count
			1,  // maximum count
			NULL);          // unnamed semaphore
	_frontBufRead = CreateSemaphore( 
			NULL,           // default security attributes
			1,  // initial count
			1,  // maximum count
			NULL);          // unnamed semaphore
	_beginthreadex (
			NULL,              // default security attributes
			0,                 // use default stack size  
			readFrameThreadEntry,        // thread function 
			this,             // argument to thread function 
			0,                 // use default creation flags 
			NULL);   // returns the thread identifier 
}

SensorDataFileInput::~SensorDataFileInput () {
	delete []_buf;
	CloseHandle (_frontBufRead);
	CloseHandle (_frontBufReady);
}


bool SensorDataFileInput::_grabFrame ()
{
	clock_t start = clock ();
	WaitForSingleObject (_frontBufReady, INFINITE);
	clock_t afterWait = clock ();
	bool result = false;
	if (_hasNewFrame) {
		if (__depthMetaData) {
			memcpy (__depthMetaData->imageData, _frontBuf,
					__sensorParameter.frameSize *
					__sensorParameter.depthPixelWidth);
			for (unsigned int v = 0; v < __sensorParameter.frameHeight; ++v)
				for (unsigned int u = 0; u < __sensorParameter.frameWidth; ++u)
					if (CV_IMAGE_ELEM (__depthMetaData, unsigned short, v, u) == 0)
						CV_IMAGE_ELEM (__depthMetaData, unsigned short, v, u) =
								__sensorParameter.frameDepth;
		}
		if (__rgbMetaData) {
			memcpy (__rgbMetaData->imageData,
					_frontBuf + __sensorParameter.frameSize *
					__sensorParameter.depthPixelWidth,
					__sensorParameter.frameSize *
					__sensorParameter.rgbPixelWidth);
		}
		result = true;
	}
	ReleaseSemaphore (_frontBufRead, 1, NULL);
	clock_t end = clock ();
	std::ostringstream sOut;
	sOut << getName () << ": New frame grabbed [wait=" << afterWait - start <<
			"][copy=" << end - afterWait << "]" << std::endl;
	OutputDebugStringA (sOut.str ().c_str ());
	return result;
}

void SensorDataFileInput::backThreadMain ()
{
	while (true) {
		std::ostringstream sOut;
		clock_t start = clock ();
		_inStream.read (_backBuf, _frameDataSize);
		if (_inStream)
			_hasNewFrame = true;
		clock_t end = clock ();
		sOut << getName () << ": New frame read: " << end - start << std::endl;
		OutputDebugStringA (sOut.str ().c_str ());
		WaitForSingleObject (_frontBufRead, INFINITE);
		char *tempPtr = _frontBuf;
		_frontBuf = _backBuf;
		_backBuf = tempPtr;
		ReleaseSemaphore (_frontBufReady, 1, NULL);
	}
}

unsigned int WINAPI readFrameThreadEntry (void * lpParam)
{
	((SensorDataFileInput *)lpParam)->backThreadMain ();
	return 0;
}
